﻿using System;
using System.Collections.Generic;
using System.ComponentModel;
using Microsoft.Ccr.Core;
using Microsoft.Dss.Core.Attributes;
using Microsoft.Dss.ServiceModel.Dssp;
using Microsoft.Dss.ServiceModel.DsspServiceBase;
using Microsoft.Robotics.PhysicalModel;
using W3C.Soap;
using webcam = Microsoft.Robotics.Services.WebCam.Proxy;
using lrf = Microsoft.Robotics.Services.Sensors.SickLRF.Proxy;
using drive = Microsoft.Robotics.Services.Drive.Proxy;
using System.Drawing;

namespace SimPioneersControl
{
    /// <summary>
    /// SimPioneersControl State class
    /// </summary>
    [DataContract]
    public class State
    {
        #region TT Maze State Variables
        private Maze maze = new Maze();

        [DataMember]
        [Description("The objects related to building the maze")]
        public Maze Maze
        {
            get { return maze; }
            set { maze = value; }
        }

        // Wall Colors
        // These are specified using the Vector3 format for colors
        // The values below correspond to the bmp files above
        static public readonly Vector3[] DefaultColors =
        {
            new Vector3(  0,   0,   0),
            new Vector3(255,   0,   0),
            new Vector3(  0, 255,   0),
            new Vector3(255, 255,   0),
            new Vector3(  0,   0, 255),
            new Vector3(255,   0, 255),
            new Vector3(  0, 255, 255),
            new Vector3(255, 255, 255),
            new Vector3(128, 128, 128),
            new Vector3(128,   0,   0),
            new Vector3(  0, 128,   0),
            new Vector3(128, 128,   0),
            new Vector3(  0,   0, 128),
            new Vector3(128,   0, 128),
            new Vector3(  0, 128, 128),
            new Vector3(192, 192, 192)
        };
        #endregion 

        #region Map Variables
        private Map map = new Map();

        [DataMember]
        [Description("The objects related to mapping")]
        public Map Map
        {
            get { return map; }
            set { map = value; }
        }

        #endregion

        #region Robot State Variables
        //It is defined +1 since we find better to work with numbers different from 0, the 0 is left null
        private Robot[] p3dx = new Robot[SimPioneersControlService.num_of_robots+1];  

        public void Initialize()
        {
            for (int i = 1; i < p3dx.Length; i++)
                p3dx[i] = new Robot();                
        }

        [DataMember]
        [Description("Vector of robotic entities containing all data members included in Robot class")]
        public Robot[] P3DX
        {
            get { return p3dx; }
            set { p3dx = value; }
        }
        
        #endregion
    }


    /// <summary>
    /// This class is for organization purposes, and must be instantiated in State class for each robot in PopulateWorld()
    /// </summary>
    [DataContract]
    public class Robot
    {
        #region High-level
        Task _Task = Task.Explore_Map;
        TaskStatus _TaskStatus = TaskStatus.InProcess;
        GlobalStatus _GlobalStatus = GlobalStatus.Idle;
        Role _Role = Role.PoliceForce;
        private string _HighLvl_Command = "";
        private int _ID;
   

        [DataMember]
        [Description("Contains enum value for the current active task")]
        public Task Task
        {
            get { return _Task; }
            set { _Task = value; }
        }

        [DataMember]
        [Description("Contains enum value for the current active task's status")]
        public TaskStatus TaskStatus
        {
            get { return _TaskStatus; }
            set { _TaskStatus = value; }
        }

        [DataMember]
        [Description("Contains enum value for the current global/overall robot status")]
        public GlobalStatus GlobalStatus
        {
            get { return _GlobalStatus; }
            set { _GlobalStatus = value; }
        }

        [DataMember]
        [Description("Contains enum value for the current active role")]
        public Role Role
        {
            get { return _Role; }
            set { _Role = value; }
        }

        [DataMember]
        [Description("For System element communications in a JAUS-like architecture")]
        public string HighLvl_Command
        {
            get { return _HighLvl_Command; }
            set { _HighLvl_Command = value; }
        }

        [DataMember]
        [Description("For identyfying the robot")]
        public int ID
        {
            get { return _ID; }
            set { _ID = value; }
        }

        public void ResetStatus()
        {
            TaskStatus = TaskStatus.InProcess;
            GlobalStatus = GlobalStatus.Idle;
            FSMstate = OperationsState.Idle;
            HighLvl_Command = "";
        }

        #endregion

        #region Low-level
        ActiveBehavior _Behavior = ActiveBehavior.SafeWander;
        OperationsState _FSMstate;
        private int _Behavior_Count = 0;
        private int _Task_Count = 0;
        private drive.DriveDifferentialTwoWheelState _DriveState = new drive.DriveDifferentialTwoWheelState();
        private bool _DriveInProcess = false;
        private bool _InGoal = false;
        private bool _IsDocked = false;
        private Waypoints _waypoints =  new Waypoints();
       

        [DataMember]
        [Description("Contains enum value for the current active robot behavior")]
        public ActiveBehavior Behavior
        {
            get { return _Behavior; }
            set { _Behavior = value; }
        }

        [DataMember]
        [Description("Contains enum value for the current active robot fsm state")]
        public OperationsState FSMstate
        {
            get { return _FSMstate; }
            set { _FSMstate = value; }
        }

        [DataMember]
        [Description("For measuring the constant steps using the same active behavior")]
        public int Behavior_Count
        {
            get { return _Behavior_Count; }
            set { _Behavior_Count = value; }
        }

        [DataMember]
        [Description("For measuring the constant steps using the same active task")]
        public int Task_Count
        {
            get { return _Task_Count; }
            set { _Task_Count = value; }
        }

        [DataMember]
        [Description("For determining the drive status: basically is moving, enabled or stopped")]
        public drive.DriveDifferentialTwoWheelState DriveState
        {
            get { return _DriveState; }
            set { _DriveState = value; }
        }

        [DataMember]
        [Description("For determining if the drive is pending a turning or translating operation")]
        public bool DriveInProcess
        {
            get { return _DriveInProcess; }
            set { _DriveInProcess = value; }
        }

        [DataMember]
        [Description("For determining if going towards a location has been achieved")]
        public bool InGoal
        {
            get { return _InGoal; }
            set { _InGoal = value; }
        }

        [DataMember]
        [Description("For determining if the robot is in formation required position")]
        public bool IsDocked
        {
            get { return _IsDocked; }
            set { _IsDocked = value; }
        }

        public void ResetVars()
        {
            Behavior_Count = 0;
            Task_Count = 0;
            DriveInProcess = false;
            InGoal = false;
            IsDocked = false;
        }

        [DataMember]
        [Description("For determining the traversed path and having a count of the times passed over it")]
        public Waypoints Waypoints
        {
            get { return _waypoints; }
            set { _waypoints = value; }
        }

        #endregion

        #region Sensors
        private Sensors _Sensors = new Sensors();

        [DataMember]
        [Description("For allocating the webcam last frame")]
        public Sensors Sensors
        {
            get { return _Sensors; }
            set { _Sensors = value; }
        }

        
        #endregion

        #region Pose
        private Physics _Physics = new Physics();

        [DataMember]
        [Description("This includes orientation, position and velocity")]
        public Physics Pose
        {
            get { return _Physics; }
            set { _Physics = value; }
        }
        #endregion

        #region Flags
        private Flags _Flags = new Flags();

        [DataMember]
        [Description("This contains all the flags for triggering behaviors")]
        public Flags Flags
        {
            get { return _Flags; }
            set { _Flags = value; }
        }

        public void ResetFlags()
        {
            Flags.AcquiescentRequest = false;
            Flags.AggregateRequest = false;
            Flags.AidAlertHumansRequest = false;
            Flags.AidDisplayInfoRequest = false;
            Flags.AidExtinguishFireRequest = false;
            Flags.AidTransportRequest = false;
            Flags.ColumnFormRequest = false;
            Flags.DiamondFormRequest = false;
            Flags.DisperseRequest = false;
            Flags.DriveTowardsRequest = false;
            Flags.EscapeRequest = false;
            Flags.FlockIsReady = false;
            Flags.FieldCoverIsReady = false;
            Flags.FieldCoverRequest = false;
            Flags.FlockRequest = false;
            Flags.GiveAidRequest = false;
            Flags.HandleCollisionRequest = false;
            Flags.HoldFormationRequest = false;
            Flags.InLocalMinima = false;
            Flags.InsideRoom = false;
            Flags.InspectRequest = false;
            Flags.KinHelpRequest = false;
            Flags.LineFormRequest = false;
            Flags.LocateRequest = false;
            Flags.LostRequest = false;
            Flags.PathPlanRequest = false;
            Flags.ReportRequest = false;
            Flags.ResumeRequest = false;
            Flags.SafeWanderRequest = false;
            Flags.SeekRequest = false;
            Flags.SysError = false;
            Flags.ThreatDetected = false;
            Flags.TrackRequest = false;
            Flags.VictimDetected = false;
            Flags.WaitRequest = false;
            Flags.WakeUpRequest = false;
            Flags.WallFollowRequest = false;
            Flags.WedgeFormRequest = false;
            Flags.WaypointsOverload = false;
        }
        
        #endregion
    }

    /// <summary>
    /// For better organization, this contains all the objects related to mapping.
    /// </summary>
    [DataContract]
    public class Map
    {
        [DataMember]
        public double MapWidth = 20;
        [DataMember]
        public double MapHeight = 20;
        [DataMember]
        public double MapResolution = 0.15;
        [DataMember]
        public double MapMaxRange = 7.99;
        [DataMember]
        public bool DrawMap = true;
        [DataMember]
        public MapMode DrawingMode = MapMode.Counting;
    }

    /// <summary>
    /// For better organization, this contains all the objects related to building the maze.
    /// </summary>
    [DataContract]
    public class Maze
    {
        // TT Feb-2007 - Updated because it was forcing the Maze to be
        // the one specified below. Same for the GroundTexture.
        [DataMember]
        public string MazeFile;

        [DataMember]
        public string GroundTexture;

        // Wall Textures
        //
        // This table of filenames maps the Basic Colors to textures
        // that can be used. A set of files with solid colors are
        // provided, but you can actually use any image file you like.
        // However, you will only see the basic color from the file
        // on the boxes. Not sure why. Probably need stretching.
        //
        // Note that Lime is full green, whereas Green is dark green.
        // Otherwise the color names are what you would expect.
        //
        // IMPORTANT NOTE: All of these files MUST be in the
        // store/media/Maze_Textures directory under your MRDS
        // installation! Otherwise your walls will disappear!
        // Also, there must be SIXTEEN (16) files in the list.
        // The code does not do any bounds checking!
        // However, you can put an empty string in as an entry,
        // in which case the color will be used instead.
        //
        [DataMember]
        public string[] WallTextures =
        {
            "Maze_Textures/SolidBlack.bmp",
            "Maze_Textures/SolidRed.bmp",
            "Maze_Textures/SolidLime.bmp",
            "Maze_Textures/SolidYellow.bmp",
            "Maze_Textures/SolidBlue.bmp",
            "Maze_Textures/SolidMagenta.bmp",
            "Maze_Textures/SolidCyan.bmp",
            "Maze_Textures/SolidWhite.bmp",
            "Maze_Textures/SolidDarkGrey.bmp",
            "Maze_Textures/SolidMaroon.bmp",
            "Maze_Textures/SolidGreen.bmp",
            "Maze_Textures/SolidOlive.bmp",
            "Maze_Textures/SolidNavy.bmp",
            "Maze_Textures/SolidPurple.bmp",
            "Maze_Textures/SolidCobalt.bmp",
            "Maze_Textures/SolidGrey.bmp"
        };


        [DataMember]
        public Vector3[] WallColors;

        // Mapping of Colors to Heights
        //
        // NOTE: There should be an element in the array here for
        // every value in the BasicColor enum!
        // Also, the background color will ALWAYS be ignored
        // regardless of its height.
        // And finally, there is no reason why a height has to be big.
        // For example, 0.000001f is not zero, so it will still be
        // used to generate boxes. With an appropriate texture, it
        // could be a pool of water (rectangular though), or some
        // other material on the floor.
        //
        [DataMember]
        public float[] HeightMap =
            {
                3.0f,
                5.0f,
                10.0f,
                8.0f,
                10.0f,
                15.0f,
                15.0f,
                20.0f,
                0.0f,
                5.0f,
                5.0f,
                5.0f,
                5.0f,
                5.0f,
                5.0f,
                5.0f,
            };

        // Mapping of Colors to Weights (Mass)
        //
        // NOTE:
        // For walls, you should specify a mass of zero. This really
        // means infinite, so the walls cannot be moved.
        //
        [DataMember]
        public float[] MassMap =
            {
                0.0f,
                0.0f,
                0.0f,
                0.0f,
                0.0f,
                0.0f,
                0.0f,
                0.0f,
                0.0f,
                0.0f,
                0.0f,
                0.0f,
                0.0f,
                0.0f,
                0.0f,
                0.0f,
            };

        [DataMember]
        public bool[] UseSphere =
            {
                false,
                false,
                false,
                false,
                false,
                false,
                false,
                false,
                false,
                false,
                false,
                false,
                false,
                false,
                false,
                false
            };

        // Camera Parameters
        [DataMember]
        public Vector3 CameraPosition = new Vector3(0, 0.5f, 0);

        [DataMember]
        public float CameraTiltAngle = 0;

        // Maze Parameters
        [DataMember]
        public float WallBoxSize = 0.98f;   // size of the wall cubes (meters)
        // Grid size for bitmaps, i.e. one pixel = GridSpacing
        [DataMember]
        public float GridSpacing = 0.1f;    // Maze grid size (meters)

        // Scale factor applied to heights
        [DataMember]
        public float HeightScale = 0.1f;
        [DataMember]
        public float DefaultHeight = 10.0f;
        // Scale factor applied to spheres
        [DataMember]
        public float SphereScale = 0.1f;

        [DataMember]
        public float BounceThreshold = 0;

        // Wall Colors
        // These are specified using the Vector3 format for colors
        // The values below correspond to the bmp files above
        static public readonly Vector3[] DefaultColors =
        {
            new Vector3(  0,   0,   0),
            new Vector3(255,   0,   0),
            new Vector3(  0, 255,   0),
            new Vector3(255, 255,   0),
            new Vector3(  0,   0, 255),
            new Vector3(255,   0, 255),
            new Vector3(  0, 255, 255),
            new Vector3(255, 255, 255),
            new Vector3(128, 128, 128),
            new Vector3(128,   0,   0),
            new Vector3(  0, 128,   0),
            new Vector3(128, 128,   0),
            new Vector3(  0,   0, 128),
            new Vector3(128,   0, 128),
            new Vector3(  0, 128, 128),
            new Vector3(192, 192, 192)
        };

        // Constructor for the State
        public void State()
        {
            int i;
            // Initialize the wall colors
            WallColors = new Vector3[16];
            for (i=0; i<16; i++)
                WallColors[i] = new Vector3(DefaultColors[i].X,
                                        DefaultColors[i].Y,
                                        DefaultColors[i].Z);
        }
    }

    ///<summary>
    /// For better organization, these variables are objects instantiated in Robot() class
    /// </summary>
    #region Robot State Variables
    /// <summary>
    /// LIST OF THE ELIGIBLE TASKS
    /// This can be useful for the FANOUT metric, substracting 'score' each time unknown is active.
    /// </summary>
    [DataContract]
    public enum Task
    {
        Unknown,
        Explore_Map,
        Recognize_Identify,
        Support_Relief,
    }

    /// <summary>
    /// LIST OF THE TASKS' STATUS
    /// This can be useful for the metrics TASK TIME DEVELOPMENT in collaboration with Data Member TaskCountdown
    /// </summary>
    [DataContract]
    public enum TaskStatus
    {
        Unknown,
        Halted,
        InProcess,
        NeedHelp,

    }

    /// <summary>
    /// LIST OF THE ELIGIBLE GLOBAL STATUS
    /// </summary>
    [DataContract]
    public enum GlobalStatus
    {
        Unknown,
        Busy,
        Idle,
        Helping
    }

    /// <summary>
    /// LIST OF THE ELIGIBLE ROLES
    /// </summary>
    [DataContract]
    public enum Role
    {
        Unknown,
        FireBrigade,            //For damage control, relief and support
        AmbulanceTeam,          //For civilian transport, relief and support
        PoliceForce,            //Clearing blockades, mapping, exploring
        Rescuer,                //Aid kin by pushing or going towards,
        Trapped,                //Asking help, Malfunction, Notifying a physical problem.
    }

    /// <summary>
    /// LIST OF THE ELIGIBLE BEHAVIORS
    /// </summary>
    [DataContract]
    public enum ActiveBehavior
    {
        WakeUp,
        Resume,
        Wait,
        HandleCollision,
        AvoidPast,
        Locate,
        DriveTowards,
        SafeWander,
        Seek,
        PathPlanning,
        Aggregate,
        UCLine,
        UCColumn,
        UCDiamond,
        UCWedge,
        HoldFormation,
        Lost,
        Flock,
        Disperse,
        FieldCover,
        WallFollow,
        Escape,
        Report,
        Track,
        Inspect,
        Victim,
        Threat,
        Kin,
        GiveAid,
        Aid_Transport,
        Aid_DisplayInfo,
        Aid_ExtinguishFire,
        Aid_AlertHumans,
        Impatient,
        Acquiescent,
        Unknown
    }

    /// <summary>
    /// LIST OF STATES FOR THE FSM
    /// This must match the sequence diagrams of each operation
    /// </summary>
    [DataContract]
    public enum OperationsState
    {
        Idle,
        Wandering,
        Aggregated,
        Forming,
        FlockingReady,
        Dispersed, 
        FieldCoverReady,
        WallFollowing
    }

    [DataContract]
    public class Physics
    {
        [DataMember]
        public double Speed;
        [DataMember]
        public double Heading;
        [DataMember]
        public double X_pose;
        [DataMember]
        public double Z_pose;  //This Z is because the simulator uses the horizontal plane as XZ.
        [DataMember]
        public Vector3 PositionVector;
        [DataMember]
        public Vector3 VelocityVector;
        [DataMember]
        public Quaternion OrientationVector;
        [DataMember]
        public double Mass;
        [DataMember]
        public Vector2 ID2Pose = new Vector2();

    }

    [DataContract]
    public class Sensors
    {
        //For allocating the Frame Image RGB Raw Data, Image Format, Size, and TimeStamp
        [DataMember]
        public webcam.QueryFrameResponse WebcamData;

        //For allocating the Frame in Bitmap Format.
        [DataMember]
        public Bitmap WebcamBitmap;

        //For allocating the whole LRF state including DistanceMeasurements, AngularRange, Resolution and TimeStamp
        [DataMember]
        public lrf.State LRF;

        [DataMember]
        public bool BumperPressed;

    }

    [DataContract]
    public class Flags
    {
        [DataMember]
        public bool WakeUpRequest = false;
        [DataMember]
        public bool ResumeRequest = false;
        [DataMember]
        public bool WaitRequest = false;
        [DataMember]
        public bool HandleCollisionRequest = false;
        [DataMember]
        public bool WaypointsOverload = false;
        [DataMember]
        public bool LocateRequest = false;
        [DataMember]
        public bool DriveTowardsRequest = false;
        [DataMember]
        public bool SafeWanderRequest = false;
        [DataMember]
        public bool SeekRequest = false;
        [DataMember]
        public bool PathPlanRequest = false;
        [DataMember]
        public bool EnoughMap = false;
        [DataMember]
        public bool AggregateRequest = false;
        [DataMember]
        public bool LineFormRequest = false;
        [DataMember]
        public bool ColumnFormRequest = false;
        [DataMember]
        public bool DiamondFormRequest = false;
        [DataMember]
        public bool WedgeFormRequest = false;
        [DataMember]
        public bool HoldFormationRequest = false;
        [DataMember]
        public bool LostRequest = false;
        [DataMember]
        public bool FlockRequest = false;
        [DataMember]
        public bool FlockIsReady = false;
        [DataMember]
        public bool DisperseRequest = false;
        [DataMember]
        public bool FieldCoverRequest = false;
        [DataMember]
        public bool FieldCoverIsReady = false;
        [DataMember]
        public bool WallFollowRequest = false;
        [DataMember]
        public bool InsideRoom = false;
        [DataMember]
        public bool InLocalMinima = false;
        [DataMember]
        public bool EscapeRequest = false;
        [DataMember]
        public bool ReportRequest = false;
        [DataMember]
        public bool TrackRequest = false;
        [DataMember]
        public bool InspectRequest = false;
        [DataMember]
        public bool VictimDetected = false;
        [DataMember]
        public bool ThreatDetected = false;
        [DataMember]
        public bool KinHelpRequest = false;
        [DataMember]
        public bool GiveAidRequest = false;
        [DataMember]
        public bool AidTransportRequest = false;
        [DataMember]
        public bool AidDisplayInfoRequest = false;
        [DataMember]
        public bool AidExtinguishFireRequest = false;
        [DataMember]
        public bool AidAlertHumansRequest = false;
        [DataMember]
        public bool AcquiescentRequest = false;
        [DataMember]
        public bool SysError = false;
        
    }

    [DataContract]
    public class Waypoints
    {
        [DataMember]
        public DateTime LastTime = new DateTime();

        [DataMember]
        public List<Points> Points = new List<Points>();

        [DataMember]
        public Tuple<bool, int> LastPoint = new Tuple<bool, int>(); 
    }

    [DataContract]
    public class Points
    {
        [DataMember]
        public int X;

        [DataMember]
        public int Z;

        [DataMember]
        public int Steps;
    }
    #endregion
}
